// --------------------------------------------------------------------------------------------------------------------
// <copyright file="HomogenousTransform.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Numerics
{
    using System;

    /// <summary>
    /// Class to create and work with homogenous transforms to translate between different coordinate frames
    /// </summary>
    public static class HomogenousTransform
    {
        /// <summary>
        /// Length of a frame array.
        /// </summary>
        public const int FrameArrayLength = 12;

        /// <summary>
        /// Builds a homogenous transform that rotates around the x-axis
        /// </summary>
        /// <param name="angle">Angle to rotate</param>
        /// <returns>Homogenous transform</returns>
        public static Matrix4 CreateFromRotationX(double angle)
        {
            Matrix4 rotationX = new Matrix4();
            rotationX[0, 0] = 1.0;
            rotationX[1, 1] = Math.Cos(angle);
            rotationX[1, 2] = -Math.Sin(angle);
            rotationX[2, 1] = Math.Sin(angle);
            rotationX[2, 2] = Math.Cos(angle);
            rotationX[3, 3] = 1.0;
            return rotationX;
        }

        /// <summary>
        /// Builds a homogenous transform that rotates around the y-axis
        /// </summary>
        /// <param name="angle">Angle to rotate</param>
        /// <returns>Homogenous transform</returns>
        public static Matrix4 CreateFromRotationY(double angle)
        {
            Matrix4 rotationY = new Matrix4();
            rotationY[0, 0] = Math.Cos(angle);
            rotationY[0, 2] = Math.Sin(angle);
            rotationY[1, 1] = 1.0;
            rotationY[2, 0] = -Math.Sin(angle);
            rotationY[2, 2] = Math.Cos(angle);
            rotationY[3, 3] = 1.0;
            return rotationY;
        }

        /// <summary>
        /// Builds a homogenous transform that rotates around the z-axis
        /// </summary>
        /// <param name="angle">Angle to rotate</param>
        /// <returns>Homogenous transform</returns>
        public static Matrix4 CreateFromRotationZ(double angle)
        {
            Matrix4 rotationZ = new Matrix4();
            rotationZ[0, 0] = Math.Cos(angle);
            rotationZ[0, 1] = -Math.Sin(angle);
            rotationZ[1, 0] = Math.Sin(angle);
            rotationZ[1, 1] = Math.Cos(angle);
            rotationZ[2, 2] = 1.0;
            rotationZ[3, 3] = 1.0;
            return rotationZ;
        }

        /// <summary>
        /// Builds a homogenous transform from an unit quaternion representing a rotation
        /// </summary>
        /// <param name="q">Unit quaternion</param>
        /// <returns>Homogenous transform</returns>
        public static Matrix4 CreateFromQuaternion(Quaternion q)
        {
            Matrix4 ht = new Matrix4();

            ht[0, 0] = (q.W * q.W) + (q.X * q.X) - (q.Y * q.Y) - (q.Z * q.Z);
            ht[0, 1] = 2.0 * ((q.X * q.Y) - (q.W * q.Z));
            ht[0, 2] = 2.0 * ((q.X * q.Z) + (q.W * q.Y));
            ht[0, 3] = 0;
            ht[1, 0] = 2.0 * ((q.X * q.Y) + (q.W * q.Z));
            ht[1, 1] = (q.W * q.W) - (q.X * q.X) + (q.Y * q.Y) - (q.Z * q.Z);
            ht[1, 2] = 2.0 * ((q.Y * q.Z) - (q.W * q.X));
            ht[1, 3] = 0;
            ht[2, 0] = 2.0 * ((q.X * q.Z) - (q.W * q.Y));
            ht[2, 1] = 2.0 * ((q.Y * q.Z) + (q.W * q.X));
            ht[2, 2] = (q.W * q.W) - (q.X * q.X) - (q.Y * q.Y) + (q.Z * q.Z);
            ht[2, 3] = 0.0;
            ht[3, 0] = 0.0;
            ht[3, 1] = 0.0;
            ht[3, 2] = 0.0;
            ht[3, 3] = 1.0;

            return ht;
        }

        /// <summary>
        /// Builds a translation homogenous transform using specified offsets
        /// </summary>
        /// <param name="v">Offset vector</param>
        /// <returns>Homogenous transform</returns>
        public static Matrix4 CreateFromTranslation(Vector3 v)
        {
            Matrix4 translate = new Matrix4();
            translate[0, 0] = 1.0;
            translate[1, 1] = 1.0;
            translate[2, 2] = 1.0;
            translate[3, 3] = 1.0;

            translate[0, 3] = v.X;
            translate[1, 3] = v.Y;
            translate[2, 3] = v.Z;
            return translate;
        }

        /// <summary>
        /// Create a homogeneous transform from an axis-angle rotation.  Axis must have a length of 1.
        /// </summary>
        /// <param name="rotation">Axis-angle representing desired rotation</param>
        /// <returns>Homogeneous transform performing the equivalent axis-angle rotation</returns>
        public static Matrix4 CreateFromAxisAngle(AxisAngle rotation)
        {
            // formula from "Mechanics of Robotic Manipulation", Mason, M., 2001, pg. 46
            Matrix4 ht = new Matrix4();
            Vector3 n = rotation.Axis;
            double s = Math.Sin(rotation.Angle);
            double c = Math.Cos(rotation.Angle);

            ht[0, 0] = (n.X * n.X) + ((1.0 - (n.X * n.X)) * c);
            ht[0, 1] = (n.X * n.Y * (1.0 - c)) - (n.Z * s);
            ht[0, 2] = (n.X * n.Z * (1.0 - c)) + (n.Y * s);
            ht[0, 3] = 0.0;
            ht[1, 0] = (n.X * n.Y * (1.0 - c)) + (n.Z * s);
            ht[1, 1] = (n.Y * n.Y) + ((1 - (n.Y * n.Y)) * c);
            ht[1, 2] = (n.Y * n.Z * (1.0 - c)) - (n.X * s);
            ht[1, 3] = 0.0;
            ht[2, 0] = (n.X * n.Z * (1.0 - c)) - (n.Y * s);
            ht[2, 1] = (n.Y * n.Z * (1.0 - c)) + (n.X * s);
            ht[2, 2] = (n.Z * n.Z) + ((1.0 - (n.Z * n.Z)) * c);
            ht[2, 3] = 0.0;
            ht[3, 0] = ht[3, 1] = ht[3, 2] = 0.0;
            ht[3, 3] = 1.0;

            return ht;
        }

        /// <summary>
        /// Create a transform with both a rotation and a translation
        /// </summary>
        /// <param name="rotation">The 3x3 rotation matrix</param>
        /// <param name="translation">The translation vector</param>
        /// <returns>The resulting Matrix4</returns>
        public static Matrix4 CreateTransformation(Matrix3 rotation, Vector3 translation)
        {
            Matrix4 result = new Matrix4();
            result[0, 0] = rotation[0, 0];
            result[0, 1] = rotation[0, 1];
            result[0, 2] = rotation[0, 2];
            result[0, 3] = translation.X;

            result[1, 0] = rotation[1, 0];
            result[1, 1] = rotation[1, 1];
            result[1, 2] = rotation[1, 2];
            result[1, 3] = translation.Y;

            result[2, 0] = rotation[2, 0];
            result[2, 1] = rotation[2, 1];
            result[2, 2] = rotation[2, 2];
            result[2, 3] = translation.Z;

            result[3, 0] = 0.0;
            result[3, 1] = 0.0;
            result[3, 2] = 0.0;
            result[3, 3] = 1.0;

            return result;
        }

        /// <summary>
        /// Create a homogenous transform that rotates from vecA to vecB.  Vectors
        /// are treated as indicating direction only, and translational component of
        /// generated transform will be zero.
        /// </summary>
        /// <param name="vecA">3 dimensional vector indicating initial rotation.</param>
        /// <param name="vecB">3 dimensional vector indicating final rotation.</param>
        /// <returns>A homogenous transform rotating vecA to vecB.</returns>
        public static Matrix4 CreateFromTwoVectors(Vector3 vecA, Vector3 vecB)
        {
            vecA = Vector3.Normalize(vecA);
            vecB = Vector3.Normalize(vecB);
            Vector3 axis = Vector3.Cross(vecA, vecB);
            axis = Vector3.Normalize(axis);
            return HomogenousTransform.CreateFromAxisAngle(new AxisAngle(axis, Math.Acos(Vector3.Dot(vecA, vecB))));
        }

        /// <summary>
        /// Applies a homogenous transform to a vector.  In both parameters and return value
        /// the '1' in the 4th row of the vector is omitted.  Assumed that last row of ht
        /// is [0 0 0 1], though this is not verified.
        /// </summary>
        /// <param name="ht">Homogenous transform</param>
        /// <param name="vec">Vector to be transformed</param>
        /// <returns>Transformed vector.</returns>
        public static Vector3 Apply(Matrix4 ht, Vector3 vec)
        {
            return new Vector3(
                (ht[0, 0] * vec.X) + (ht[0, 1] * vec.Y) + (ht[0, 2] * vec.Z) + ht[0, 3],
                (ht[1, 0] * vec.X) + (ht[1, 1] * vec.Y) + (ht[1, 2] * vec.Z) + ht[1, 3],
                (ht[2, 0] * vec.X) + (ht[2, 1] * vec.Y) + (ht[2, 2] * vec.Z) + ht[2, 3]);
        }

        /// <summary>
        /// Create a homogenous transform to convert to coordinate frame defined by pose to coordinate frame at origin.
        /// </summary>
        /// <param name="pose">Pose defining a coordinate frame relative to origin</param>
        /// <returns>Homogenous transform</returns>
        public static Matrix4 CreateFromPose(Pose pose)
        {
            Matrix4 ht = CreateFromQuaternion(pose.Orientation);
            ht = CreateFromTranslation(pose.Position) * ht;

            return ht;
        }

        /// <summary>
        /// Create a homogenous transform based on sequential rotations around z, y, x
        /// </summary>
        /// <param name="z">angle around z axis</param>
        /// <param name="y">angle around y axis</param>
        /// <param name="x">angle around x axis</param>
        /// <returns>Homogenous transform</returns>
        public static Matrix4 CreateFromEulerAngleZYX(double z, double y, double x)
        {
            Matrix4 rotz = HomogenousTransform.CreateFromRotationZ(z);
            Matrix4 roty = HomogenousTransform.CreateFromRotationY(y);
            Matrix4 rotx = HomogenousTransform.CreateFromRotationX(x);
            return rotz * roty * rotx;
        }

        /// <summary>
        /// Multiplies two homogenous transforms.  Same as standard matrix multiplication, but
        /// assumes that the last row of operands and result is [0 0 0 1].
        /// </summary>
        /// <param name="left">Left operand</param>
        /// <param name="right">Right operand</param>
        /// <returns>Result of multiplying <c>left</c> and <c>right</c></returns>
        public static Matrix4 Multiply(Matrix4 left, Matrix4 right)
        {
            Matrix4 product = new Matrix4();
            product[0, 0] = (left[0, 0] * right[0, 0]) + (left[0, 1] * right[1, 0]) + (left[0, 2] * right[2, 0])
                            + (left[0, 3] * right[3, 0]);
            product[0, 1] = (left[0, 0] * right[0, 1]) + (left[0, 1] * right[1, 1]) + (left[0, 2] * right[2, 1])
                            + (left[0, 3] * right[3, 1]);
            product[0, 2] = (left[0, 0] * right[0, 2]) + (left[0, 1] * right[1, 2]) + (left[0, 2] * right[2, 2])
                            + (left[0, 3] * right[3, 2]);
            product[0, 3] = (left[0, 0] * right[0, 3]) + (left[0, 1] * right[1, 3]) + (left[0, 2] * right[2, 3])
                            + (left[0, 3] * right[3, 3]);

            product[1, 0] = (left[1, 0] * right[0, 0]) + (left[1, 1] * right[1, 0]) + (left[1, 2] * right[2, 0])
                            + (left[1, 3] * right[3, 0]);
            product[1, 1] = (left[1, 0] * right[0, 1]) + (left[1, 1] * right[1, 1]) + (left[1, 2] * right[2, 1])
                            + (left[1, 3] * right[3, 1]);
            product[1, 2] = (left[1, 0] * right[0, 2]) + (left[1, 1] * right[1, 2]) + (left[1, 2] * right[2, 2])
                            + (left[1, 3] * right[3, 2]);
            product[1, 3] = (left[1, 0] * right[0, 3]) + (left[1, 1] * right[1, 3]) + (left[1, 2] * right[2, 3])
                            + (left[1, 3] * right[3, 3]);

            product[2, 0] = (left[2, 0] * right[0, 0]) + (left[2, 1] * right[1, 0]) + (left[2, 2] * right[2, 0])
                            + (left[2, 3] * right[3, 0]);
            product[2, 1] = (left[2, 0] * right[0, 1]) + (left[2, 1] * right[1, 1]) + (left[2, 2] * right[2, 1])
                            + (left[2, 3] * right[3, 1]);
            product[2, 2] = (left[2, 0] * right[0, 2]) + (left[2, 1] * right[1, 2]) + (left[2, 2] * right[2, 2])
                            + (left[2, 3] * right[3, 2]);
            product[2, 3] = (left[2, 0] * right[0, 3]) + (left[2, 1] * right[1, 3]) + (left[2, 2] * right[2, 3])
                            + (left[2, 3] * right[3, 3]);

            product[3, 0] = 0.0;
            product[3, 1] = 0.0;
            product[3, 2] = 0.0;
            product[3, 3] = 1.0;

            return product;
        }

        /// <summary>
        /// Inversion specific to homogenous transform.  Should be more efficient than general inversion.
        /// </summary>
        /// <param name="ht">Homogenous transform to be inverted</param>
        /// <returns>Inverse of <c>ht</c></returns>
        public static Matrix4 Inverse(Matrix4 ht)
        {
            // Inversion operation from Introduction to Robotics, by J. Craig, 2005, pg. 36
            Matrix4 result = new Matrix4();

            // take transpose of rotational component and add known last row
            result[0, 0] = ht[0, 0];
            result[1, 0] = ht[0, 1];
            result[2, 0] = ht[0, 2];
            result[0, 1] = ht[1, 0];
            result[1, 1] = ht[1, 1];
            result[2, 1] = ht[1, 2];
            result[0, 2] = ht[2, 0];
            result[1, 2] = ht[2, 1];
            result[2, 2] = ht[2, 2];
            result[3, 0] = 0.0;
            result[3, 1] = 0.0;
            result[3, 2] = 0.0;
            result[3, 3] = 1.0;

            // can now use the inverted rotation to determine the translational component
            Vector3 transComponent = Apply(result, new Vector3(ht[0, 3], ht[1, 3], ht[2, 3]));
            result[0, 3] = -transComponent.X;
            result[1, 3] = -transComponent.Y;
            result[2, 3] = -transComponent.Z;

            return result;
        }

        /// <summary>
        /// Converts a homogenous matrix to a 12 dimensional array corresponding to
        /// the 3x4 frame represented by the given homogenous transform.
        /// </summary>
        /// <param name="ht">A homogenous transform.</param>
        /// <returns>12 dimensional frame array corresponding to the provided transform.</returns>
        public static double[] ToFrameArray(Matrix4 ht)
        {
            double[] frameArray = new double[FrameArrayLength];

            frameArray[0] = ht[0, 0];
            frameArray[1] = ht[0, 1];
            frameArray[2] = ht[0, 2];
            frameArray[3] = ht[0, 3];
            frameArray[4] = ht[1, 0];
            frameArray[5] = ht[1, 1];
            frameArray[6] = ht[1, 2];
            frameArray[7] = ht[1, 3];
            frameArray[8] = ht[2, 0];
            frameArray[9] = ht[2, 1];
            frameArray[10] = ht[2, 2];
            frameArray[11] = ht[2, 3];

            return frameArray;
        }

        /// <summary>
        /// Converts a row-major frame array into 
        /// </summary>
        /// <param name="frame">A row-major frame array.</param>
        /// <returns>A homogenous transform.</returns>
        public static Matrix4 FromFrameArray(double[] frame)
        {
            Matrix4 ht = new Matrix4();

            ht[0, 0] = frame[0];
            ht[0, 1] = frame[1];
            ht[0, 2] = frame[2];
            ht[0, 3] = frame[3];
            ht[1, 0] = frame[4];
            ht[1, 1] = frame[5];
            ht[1, 2] = frame[6];
            ht[1, 3] = frame[7];
            ht[2, 0] = frame[8];
            ht[2, 1] = frame[9];
            ht[2, 2] = frame[10];
            ht[2, 3] = frame[11];
            ht[3, 0] = 0;
            ht[3, 1] = 0;
            ht[3, 2] = 0;
            ht[3, 3] = 1;

            return ht;
        }

        /// <summary>
        /// Convert a matrix to an AxisAngle
        /// </summary>
        /// <param name="matrix">The matrix to convert</param>
        /// <returns>An AxisAngle converted, the angle is between 0 and pi</returns>
        //// Reference: http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/index.htm
        public static AxisAngle ToAxisAngle(Matrix4 matrix)
        {
            AxisAngle axisAngle;

            double diff01 = matrix[0, 1] - matrix[1, 0];
            double diff02 = matrix[0, 2] - matrix[2, 0];
            double diff12 = matrix[1, 2] - matrix[2, 1];
            double sum01 = matrix[0, 1] + matrix[1, 0];
            double sum02 = matrix[0, 2] + matrix[2, 0];
            double sum12 = matrix[1, 2] + matrix[2, 1];
            double sqrtOfTwoOverTwo = Math.Sqrt(2) / 2.0;

            // test matrix symmetry for singular cases
            // angle = 0 and angle = pi
            double localErrorEpsilon = 0.001;
            if (Math.Abs(diff01) < MathConstants.ErrorEpsilon && Math.Abs(diff02) < MathConstants.ErrorEpsilon && Math.Abs(diff12) < MathConstants.ErrorEpsilon)
            {
                // test if this is an identity matrix, if this is, then angle = 0, otherwise, angle = pi
                if (Math.Abs(sum01) < MathConstants.ErrorEpsilon &&
                    Math.Abs(sum02) < MathConstants.ErrorEpsilon &&
                    Math.Abs(sum12) < MathConstants.ErrorEpsilon &&
                    Math.Abs(matrix[0, 0] - 1) < localErrorEpsilon &&
                    Math.Abs(matrix[1, 1] - 1) < localErrorEpsilon &&
                    Math.Abs(matrix[2, 2] - 1) < localErrorEpsilon)
                {
                    // 0 degree situation
                    axisAngle = new AxisAngle(new Vector3(0, 0, 1), 0);
                }
                else
                {
                    // 180 degree situation
                    double x, y, z;
                    double xy = sum01 / 4.0;
                    double xz = sum02 / 4.0;
                    double yz = sum12 / 4.0;

                    // Computing the axis and angle for this singular situation
                    // Please note that the following procedure is not exactly the
                    // same as implemented in the reference.
                    // In the reference, for each of the following situations,
                    // the reference has an extra check to see if the largest value is actually close
                    // to -1 by checking xx (yy, zz) < epsilon as used in the reference. However
                    // this situation should never happen since no rotation matrix can have
                    // its three diagonal cells all approaching -1. Thus, this extra check
                    // for each of the following situations is not included here.
                    if (matrix[0, 0] >= matrix[1, 1] && matrix[0, 0] >= matrix[2, 2])
                    {
                        // m[0,0] is the largest diagonal term
                        x = Math.Sqrt((matrix[0, 0] + 1) / 2.0);
                        y = xy / x;
                        z = xz / x;
                    }
                    else if (matrix[1, 1] >= matrix[2, 2] && matrix[1, 1] >= matrix[0, 0])
                    {
                        // m[1,1] is the largest diagonal term
                        y = Math.Sqrt((matrix[1, 1] + 1) / 2.0);
                        x = xy / y;
                        z = yz / y;
                    }
                    else
                    {
                        // m[2,2] is the largest diagonal term
                        z = Math.Sqrt((matrix[2, 2] + 1) / 2.0);
                        x = xz / z;
                        y = yz / z;
                    }

                    // assemble the rotation axis
                    axisAngle = new AxisAngle(new Vector3(x, y, z), Math.PI);
                }
            }
            else
            {
                // general case
                double diffSqrt = Math.Sqrt(diff01 * diff01 + diff02 * diff02 + diff12 * diff12);
                axisAngle = new AxisAngle(new Vector3(-diff12 / diffSqrt, diff02 / diffSqrt, -diff01 / diffSqrt), Math.Acos(Math.Min(Math.Max((matrix[0, 0] + matrix[1, 1] + matrix[2, 2] - 1) / 2, -1), 1)));
            }

            return axisAngle;
        }

        /// <summary>
        /// Given a homogeneous transform matrix, this method makes
        /// the rotational matrix orthogonal.
        /// </summary>
        /// <param name="transform">A homogeneous transform whose rotational sub-matrix
        /// will be modified and made orthogonal</param>
        //// Reference: http://www.cg.info.hiroshima-cu.ac.jp/~miyazaki/knowledge/teche69.html
        public static void OrthogonalizeRotationalMatrix(Matrix4 transform)
        {
            // normalize this rotation matrix because the input matrix could be
            // not perfectly normalized, which happens in practice
            Vector3 a = new Vector3(transform[0, 0], transform[1, 0], transform[2, 0]);
            Vector3 b = new Vector3(transform[0, 1], transform[1, 1], transform[2, 1]);
            Vector3 c = new Vector3(transform[0, 2], transform[1, 2], transform[2, 2]);
            a = Vector3.Normalize(a);
            b = b - Vector3.Dot(b, a) * a;
            b = Vector3.Normalize(b);
            c = Vector3.Cross(a, b);

            transform[0, 0] = a.X;
            transform[1, 0] = a.Y;
            transform[2, 0] = a.Z;

            transform[0, 1] = b.X;
            transform[1, 1] = b.Y;
            transform[2, 1] = b.Z;

            transform[0, 2] = c.X;
            transform[1, 2] = c.Y;
            transform[2, 2] = c.Z;
        }
    }
}
